#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/CompleteDriver.h"

#define READNUM 3

void initializeRobot(){
  servo[frontarm] = 0;
  servo[backarm] = 244;
  servo[dispencer] = 5;
  HTSMUXinit();
  HTSMUXscanPorts(SMUX_1);
  HTSMUXscanPorts(SMUX_2);
  wait1Msec(50);
  StartTask(sensorRead);
  bDisplayDiagnostics = false;
  kalmindebugdisplay = false;
}

int differences[2][READNUM];
int speeda[2] = {60,-60};
int speedc[2] = {60,-60};
bool isdone [2] = {false,false};

task main {
  int i;
  int reader;
  int startheading;
  int averageforward = 0;
  int averagebackward = 0;
  initializeRobot();
  startkalmin();
  wait1Msec(50);
  startheading = headingwneg;
  kalmindebugdisplay = true;
  //while (true){
  //nxtDisplayTextLine (1, "Heading: %d", headingwneg);
  // }
  eraseDisplay();
  while (nNxtButtonPressed != kEnterButton);
  while (!isdone[0] ||  !isdone[1]){
    for (i = 0; i <READNUM; i++){
      startheading = headingwneg;
      motor[b] = speeda[0];
      motor[d] = speedc[0];
      wait1Msec(2500);
      motor[b] = 0;
      motor[d] = 0;
      wait1Msec(100);
      reader = headingwneg;
      writeDebugStreamLine("Startheading: " + startheading);
      writeDebugStreamLine("Endheading: " + reader);
      differences[0][i] = reader - startheading;
      averageforward += differences[0][i];
      nxtDisplayTextLine(1, "Dfwd: %d", differences [0][i]);
      startheading = headingwneg;
      motor[b] = speeda[1];
      motor[d] = speedc[1];
      wait1Msec(2500);
      motor[b] = 0;
      motor[d] = 0;
      wait1Msec(100);
      reader = headingwneg;
      writeDebugStreamLine("Startheading: " + startheading);
      writeDebugStreamLine("Endheading: " + reader);
      differences[1][i] = reader - startheading;
      averagebackward += differences[1][i];
      nxtDisplayTextLine(2, "Dbck: %d", differences [1][i]);
    }
    averageforward = averageforward / READNUM;
    averagebackward = averagebackward / READNUM;
    nxtDisplayTextLine(1, "FWDAVG: %d", averageforward);
    nxtDisplayTextLine(2, "BCKAVG: %d", averagebackward);
    if (isdone[0] || averageforward == 0){
      isdone[0] = true;
    }
    else {
    speeda[0]+= (averageforward > 0) ? (1):(-1);
    }
    if (isdone[1] || averagebackward == 0){
      isdone[1] = true;
    }
    else {
    speeda[1]+= (averagebackward > 0) ? (-1):(1);
    }
    nxtDisplayTextLine(3, "FWDMA %d", speeda[0]);
    nxtDisplayTextLine(4, "FWDMC %d", speedc[0]);
    nxtDisplayTextLine(5, "BCKMA %d", speeda[1]);
    nxtDisplayTextLine(6, "BCKMC %d", speedc[1]);
    wait1Msec(500);
  }
}
